#define _GNU_SOURCE 1
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <getopt.h>

#include "ukfCfg.h"
#include "ukfCfg1.h"

#define cfg0 (int)0
// #define cfg1 (int)1

/*---------------------------------------------*/
/*           Function Prototype                */
/*---------------------------------------------*/
// void show_matrix_obj(Matrix_t A);
// void show_matrix(double *A, int n, int m);
int ukf_test(void);
// int mtxlib_test(void);
// int mtxlib_test2(void);
// void report_compiler(void);
// FILE *stream_log;

int verbose = 0;

// int main(int argc, char *argv[])
int main()
{
	// char *fname = NULL;
	int error = 0;
	// int opt, index;

	// while ((opt = getopt(argc, argv, "vo::")) != -1)
	// 	switch(opt)
	// 	{
	// 		case 'v': verbose++; break;
	// 		case 'o':
	// 				  fname = (optarg)?strdupa(optarg):strdupa(argv[optind++]);
	// 				  break;
	// 		default:
	// 				  fprintf(stderr, "Usage: %s [-v] [-ofile]\n", argv[0]);
	// 				  exit(EXIT_FAILURE);
	// 	}
	// for (index = optind; index < argc; index++)
	// 	printf ("Non-option argument %s\n", argv[index]);
	// if (!fname)
	// 	fname = strdupa("results.txt");

	// if ((strlen(fname) < 1) || (stream_log = fopen(fname, "wt")) == 0)
	// {
	// 	perror(fname);
	// 	stream_log = stderr;
	// }
	// if (verbose)
	// {
	// 	printf("verbose = %d\n", verbose);
	// 	printf("output: %s\n", fname);
	// 	report_compiler();
	// }

	// generic matrix operation test
	// error |= mtxlib_test();

	// error |= mtxlib_test2();

	// UKF test start here
	error |= ukf_test();

	// fclose(stream_log);

	return 0;
}

int ukf_test(void)
{
	_Bool tfInitCfg0 = 0;
	// _Bool tfInitCfg1 = 0;
	UKF_t ukfIo[2];
	uint32_t simLoop;

	// UKF filter measurement input(data log is generated in matlab and used for UKF simulation for 15 iteration)
	// UKF滤波器测量输入（数据日志在matlab中生成，用于UKF仿真15次迭代）
	static const double yt[2][15] = { { 0, 16.085992708563385, 12.714829185978214, 14.528500994457660, 19.105561355310275, 23.252820029388918,
		29.282949862903255, 36.270058819651275, 44.244884173240955, 47.394243121124411, 55.988905459180458,
		61.667450941562109, 68.624980301613647, 76.337963872393104, 82.611325690835159 }, // y1 test
				 { 0, 16.750821420874981, 14.277640835870006, 16.320754051600520, 20.560460303503849, 24.827446289454556, 31.290961393448615, 36.853553457560210,
					 42.157283183453522, 49.382835230961490, 57.516319669684677, 65.664496283509095, 71.428712755732704, 79.241720894223079, 84.902760328915676 } };

	// // UKF filter expected system states calculated with matlab script for 15 iterations
	// // UKF 滤波器预期系统状态使用 matlab 脚本计算 15 次迭代
	// static const double x_exp[15][4] = {
	// /*          x1                   x2                   x3                   x4*/
	// 	{ 4.901482729572258, 4.576939885855807, 49.990342921246459, 49.958134463327802 },
	// 	{ 10.103304943868373, 9.409135720815829, 50.226544716205318, 49.750795004242228 },
	// 	{ 15.132069573131298, 14.138974122835807, 50.429540890147599, 49.191128327737864 },
	// 	{ 20.322823824348411, 19.096919763991380, 50.836860772439010, 49.189580207886742 },
	// 	{ 24.940146120267713, 23.399758647105461, 49.577386595072561, 47.383813382660449 },
	// 	{ 30.021901202161214, 27.882145089050120, 49.977568320123794, 46.551744562547626 },
	// 	{ 34.844137036519108, 32.753891693435087, 49.474006205027358, 47.190693993547214 },
	// 	{ 39.048783329419251, 38.499098203031146, 47.606199725375902, 50.001113730363919 },
	// 	{ 43.883085498256158, 42.383331307689538, 47.209657232695072, 46.747611757031784 },
	// 	{ 49.479941190207498, 47.255980559687778, 49.911944505272395, 47.887236233284476 },
	// 	{ 55.928745858553086, 51.180270357916882, 53.472542964944132, 46.510558543249353 },
	// 	{ 61.636426955126616, 55.275415649334157, 54.052126522632797, 45.262815392265203 },
	// 	{ 67.755622369652016, 59.602096868661732, 55.881393486598796, 45.326766104509289 },
	// 	{ 73.045763444967164, 63.838187852739992, 54.782159791340007, 44.291415099856643 },
	// 	{ 80.489525793047093, 66.908477563332085, 58.973616985147245, 42.638148924845950 }
	// };
	// UKF initialization: CFG0
	tfInitCfg0 = ukf_init(&ukfIo[cfg0], &UkfMatrixCfg0);

	if (tfInitCfg0 == 0)
	{
		// double err[4] = { 0, 0, 0, 0 };
		// double absErrAccum[4] = { 0, 0, 0, 0 };

		// UKF simulation CFG0: BEGIN
		for (simLoop = 1; simLoop < 15; simLoop++)
		{
			double *const py_cfg0 = ukfIo[cfg0].input.y.val;

			// UKF:CFG0 apply/load system measurements in working array for current iteration.
			py_cfg0[0] = yt[0][simLoop];
			py_cfg0[1] = yt[1][simLoop];

			// UKF:CFG0 periodic task call
			(void)ukf_step(&ukfIo[cfg0]);

			// err[0] = fabs(ukfIo[cfg0].update.x.val[0] - x_exp[simLoop - 1][0]);
			// err[1] = fabs(ukfIo[cfg0].update.x.val[1] - x_exp[simLoop - 1][1]);
			// err[2] = fabs(ukfIo[cfg0].update.x.val[2] - x_exp[simLoop - 1][2]);
			// err[3] = fabs(ukfIo[cfg0].update.x.val[3] - x_exp[simLoop - 1][3]);

			// printf("Loop: %d |system states : ukf.m | system states : est | system states : impl. diff \n", (int)simLoop);
			// printf("          %2.14f        %2.14f       %2.16f\n", x_exp[simLoop - 1][0], ukfIo[cfg0].update.x.val[0], err[0]);
			// printf("          %2.14f        %2.14f       %2.16f\n", x_exp[simLoop - 1][1], ukfIo[cfg0].update.x.val[1], err[1]);
			// printf("          %2.14f        %2.14f       %2.16f\n", x_exp[simLoop - 1][2], ukfIo[cfg0].update.x.val[2], err[2]);
			// printf("          %2.14f        %2.14f       %2.16f\n", x_exp[simLoop - 1][3], ukfIo[cfg0].update.x.val[3], err[3]);

			// printf("Loop: %d |system states : ukf.m | system states : est | system states : impl. diff \n", (int)simLoop);
			// printf("%2.15f,\n", ukfIo[cfg0].update.x.val[0]);
			// printf("%2.15f,\n", ukfIo[cfg0].update.x.val[1]);
			// printf("%2.15f,\n", ukfIo[cfg0].update.x.val[2]);
			// printf("%2.15f,\n", ukfIo[cfg0].update.x.val[3]);


			// // accumulate the differennce between reference matlab implementation and results from C code execution
			// // 对比Matlab实现和C代码执行结果之间的差异，并对它们进行累加
			// absErrAccum[0] += err[0];
			// absErrAccum[1] += err[1];
			// absErrAccum[2] += err[2];
			// absErrAccum[3] += err[3];

			printf("x.val:%2.14f.\n", 	ukfIo[cfg0].update.x.val[0]);
			// printf("x.nelem:%d.\n", 	ukfIo[cfg0].update.x.nelem);
			// printf("x.nelem:%d,nrow:%d,ncol:%d.\n", 	ukfIo[cfg0].update.x.nelem,ukfIo[cfg0].update.x.nrow,ukfIo[cfg0].update.x.ncol);
	
		}
		// printf("Accumulated error: CFG0 \n");
		// printf("%2.16f  \n%2.16f  \n%2.16f  \n%2.16f \n", absErrAccum[0], absErrAccum[1], absErrAccum[2], absErrAccum[3]);

		// UKF simulation CFG0: END
	}
	else
	{
		// initialization fail
	}
	return 0;
}